#include "StdAfx.h"
#include <I3DEngine.h>
#include <IRenderAuxGeom.h>
#include "../CharacterInstance.h"
#include "../Model.h"
#include "../ModelSkeleton.h"
#include "../CharacterManager.h"

#include "../FacialAnimation/FacialInstance.h"

#include "PoseModifierHelper.h"
#include "LookAt.h"

CRYREGISTER_CLASS(CLookAt)

CLookAt::CLookAt() :
	m_pAttachmentEyeLeft(NULL),
	m_pAttachmentEyeRight(NULL)
{
	Init();
}

CLookAt::~CLookAt()
{
}

//

void CLookAt::Init()
{
	m_UseLookIK=0;
	m_AllowAdditionalTransforms=1;
	m_LookIKFadeout=0;

	m_LookDirection=Vec3(0,1,0);	//just for debugging
	m_LookIKTarget=Vec3(ZERO);
	m_LookIKTargetSmooth=Vec3(ZERO);
	m_LookIKTargetSmoothRate=Vec3(ZERO);

	m_LookIKBlend	=0.0f;
	m_LookIKFOR		=gf_PI*0.5f;

	m_lookIKBlends[0] = 0.04f;
	m_lookIKBlends[1] = 0.06f;
	m_lookIKBlends[2] = 0.08f;
	m_lookIKBlends[3] = 0.15f;
	m_lookIKBlends[4] = 0.60f;

	m_lookIKBlendsSmooth[0] = 0.04f;
	m_lookIKBlendsSmooth[1] = 0.06f;
	m_lookIKBlendsSmooth[2] = 0.08f;
	m_lookIKBlendsSmooth[3] = 0.15f;
	m_lookIKBlendsSmooth[4] = 0.60f;
	m_lookIKBlendsSmoothRate[0] = 0.04f;
	m_lookIKBlendsSmoothRate[1] = 0.06f;
	m_lookIKBlendsSmoothRate[2] = 0.08f;
	m_lookIKBlendsSmoothRate[3] = 0.15f;
	m_lookIKBlendsSmoothRate[4] = 0.60f;

	m_oldSpine1.SetIdentity();
	m_oldSpine2.SetIdentity();
	m_oldSpine3.SetIdentity();
	m_oldNeck.SetIdentity();
	m_oldNeck1.SetIdentity();
	m_oldHead.SetIdentity();
	m_oldEyeLeft.SetIdentity();
	m_oldEyeRight.SetIdentity();

	m_additionalRotationLeft.SetIdentity();
	m_additionalRotationRight.SetIdentity();

	m_addHead.SetIdentity();
	m_addLEye.SetIdentity();
	m_addREye.SetIdentity();
}

// IAnimationPoseModifier



bool CLookAt::Prepare(const SAnimationPoseModiferParams& params)
{
	CSkeletonAnim* pSkeletonAnim = PoseModifierHelper::GetSkeletonAnim(params);
	if(pSkeletonAnim && pSkeletonAnim->m_TrackViewExclusive)
	{
		m_LookIKTargetSmooth = m_LookIKTarget;
		m_lookIKBlendsSmooth[0] = m_lookIKBlends[0];
		m_lookIKBlendsSmooth[1] = m_lookIKBlends[1];
		m_lookIKBlendsSmooth[2] = m_lookIKBlends[2];
		m_lookIKBlendsSmooth[3] = m_lookIKBlends[3];
		m_lookIKBlendsSmooth[4] = m_lookIKBlends[4];
	}
	else
	{
		SmoothCD(m_LookIKTargetSmooth, m_LookIKTargetSmoothRate, params.timeDelta, m_LookIKTarget, 0.10f);
		SmoothCD(m_lookIKBlendsSmooth[0], m_lookIKBlendsSmoothRate[0], params.timeDelta, m_lookIKBlends[0], 0.10f);
		SmoothCD(m_lookIKBlendsSmooth[1], m_lookIKBlendsSmoothRate[1], params.timeDelta, m_lookIKBlends[1], 0.10f);
		SmoothCD(m_lookIKBlendsSmooth[2], m_lookIKBlendsSmoothRate[2], params.timeDelta, m_lookIKBlends[2], 0.10f);
		SmoothCD(m_lookIKBlendsSmooth[3], m_lookIKBlendsSmoothRate[3], params.timeDelta, m_lookIKBlends[3], 0.10f);
		SmoothCD(m_lookIKBlendsSmooth[4], m_lookIKBlendsSmoothRate[4], params.timeDelta, m_lookIKBlends[4], 0.10f);
	}

	//

	m_pAttachmentEyeLeft = NULL;
	m_pAttachmentEyeRight = NULL;

	m_ql = QuatT(IDENTITY);
	m_qr = QuatT(IDENTITY);

	IAttachmentManager* pIAttachmentManager = params.pCharacterInstance->GetIAttachmentManager();

	int32 alidx = pIAttachmentManager->GetIndexByName("eye_left");
	int32 aridx = pIAttachmentManager->GetIndexByName("eye_right");
	if (alidx>=0 && aridx>=0 )
	{
		m_pAttachmentEyeLeft = SPU_MAIN_PTR( (CAttachment*)pIAttachmentManager->GetInterfaceByIndex(alidx) );
		m_pAttachmentEyeRight = SPU_MAIN_PTR( (CAttachment*)pIAttachmentManager->GetInterfaceByIndex(aridx) );
		m_pAttachmentEyeLeft->m_additionalRotation.SetIdentity();
		m_pAttachmentEyeRight->m_additionalRotation.SetIdentity();
		m_ql = QuatT(m_pAttachmentEyeLeft->CAttachment::GetAttRelativeDefault());
		m_qr = QuatT(m_pAttachmentEyeRight->CAttachment::GetAttRelativeDefault());
	}

	if (m_pAttachmentEyeLeft==0)
		return false;
	if (m_pAttachmentEyeRight==0)
		return false;

	m_pAttachmentEyeLeft->m_additionalRotation.SetIdentity();
	m_pAttachmentEyeRight->m_additionalRotation.SetIdentity();

	return true;
}

void CLookAt::Synchronize()
{
	if (m_pAttachmentEyeLeft)
	{
		m_pAttachmentEyeLeft->m_additionalRotation = m_additionalRotationLeft;
		m_pAttachmentEyeLeft->m_additionalRotation.SetNlerp(IDENTITY, m_pAttachmentEyeLeft->m_additionalRotation, m_IKBlend);
		m_pAttachmentEyeLeft = NULL;
	}

	if (m_pAttachmentEyeRight)
	{
		m_pAttachmentEyeRight->m_additionalRotation = m_additionalRotationRight;
		m_pAttachmentEyeRight->m_additionalRotation.SetNlerp(IDENTITY, m_pAttachmentEyeRight->m_additionalRotation, m_IKBlend);
		m_pAttachmentEyeRight = NULL;
	}
}



SPU_INDIRECT(CommandBufferExecute(ML))
bool CLookAt::Execute(const SAnimationPoseModiferParams& params)
{
	// transfer this to stack on SPU, to reduce lookups to member variables
	SpuStackValue<CLookAt,true,true> stackObj( *this );
	return stackObj->ExecuteLocal(params);


}
#if 1
bool CLookAt::ExecuteLocal(const SAnimationPoseModiferParams& params)
{
	ICharacterModelSkeleton* pCharacterModelSkeleton = SPU_MAIN_PTR( params.GetICharacterModelSkeleton() );
	ISkeletonPose* pSkeletonPose = SPU_MAIN_PTR( PoseModifierHelper::GetSkeletonPose(params) );

	uint32 LIKFlag = m_UseLookIK;

//	const QuatT& rPhysLocationNext = params.locationNextPhysics;
	const QuatT& rPhysLocationNext = QuatT(params.locationNextAnimation);

	CCharInstance* m_pInstance = PoseModifierHelper::GetCharInstance(params);
	CSkeletonAnim* m_pSkeletonAnim = PoseModifierHelper::GetSkeletonAnim(params);
	CModelSkeleton* m_pModelSkeleton = PoseModifierHelper::GetModelSkeleton(params);

#define BINOUT (1.0f/0.6f)

	if ( (m_pSkeletonAnim->m_IsAnimPlaying&0x1)==0)
		return false;

	if (PoseModifierHelper::GetSkeletonPose(params)->m_bInstanceVisible==0)
		return false;

	// use local pointer so spare dereferencing
	int16* 		const __restrict pIdxArray 			= m_pModelSkeleton->m_IdxArray;
	
	QuatT* 		const	__restrict	pRelPose = SPU_LOCAL_PTR( params.pPoseRelative );
	QuatT* 		const	__restrict	pAbsPose = SPU_LOCAL_PTR( params.pPoseAbsolute );	
	
	pAbsPose[0] = pRelPose[0];
	uint32 numJoints	= params.jointCount;
	for (uint32 i=1; i<numJoints; i++)
		pAbsPose[i]	= pAbsPose[m_pModelSkeleton->GetJointParentIDByID(i)] * pRelPose[i];

	Vec3 vLocalLookAtTarget = (m_LookIKTargetSmooth-rPhysLocationNext.t);

	if( Console::GetInst().ca_UseLookIK==0 || pSkeletonPose->StandingUp()>=0)
		return false;

	int32 Spine1Idx	=	pIdxArray[eIM_Spine1Idx];
	int32 Spine2Idx	=	pIdxArray[eIM_Spine2Idx];
	int32 Spine3Idx	=	pIdxArray[eIM_Spine3Idx];
	int32 NeckIdx		=	pIdxArray[eIM_NeckIdx];
	int32 HeadIdx		= pIdxArray[eIM_HeadIdx];

	int32 ridx			= pIdxArray[eIM_RightEyeIdx];
	int32 lidx			= pIdxArray[eIM_LeftEyeIdx];


	//	if (Spine1Idx<0)	return;
	//	if (Spine2Idx<0)	return;
	//	if (Spine3Idx<0)	return;
	if (NeckIdx<0)		return false;
	if (HeadIdx<0)		return false;
	if (ridx<0)				return false;
	if (lidx<0)				return false;

	PREFAST_SUPPRESS_WARNING(6255)
	QuatT* pRelativeQuatIK = SPU_LOCAL_PTR( (QuatT*)alloca( numJoints*sizeof(QuatT) ) );


#ifdef _DEBUG
	for (uint32 i=0; i<numJoints; i++)
	{
		QuatT qt = pAbsPose[i];
		assert(qt.IsValid());
	}
#endif

	//-------------------------------------------------------------------------
#if !defined(__SPU__)
	//SAuxGeomRenderFlags renderFlags( e_Def3DPublicRenderflags );
	//g_pAuxGeom->SetRenderFlags( renderFlags );
	//const char* mname = m_pInstance->m_pModel->GetFilePathCStr();
	//if ( strcmp(mname,"objects/characters/human/us/officer/officer.chr")==0 )
//	{
//	float fColor[4] = {1,1,0,1};
//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.3f, fColor, false,"LookIK: %d   %12.8f    Target:(%12.8f %12.8f %12.8f) ",m_UseLookIK,m_LookIKFOR, LookIKTarget.x, LookIKTarget.y, LookIKTarget.z );	g_YLine+=16.0f;
//	} 
#endif

	Vec3 v1		=	pAbsPose[NeckIdx  ].q.GetColumn1();
	Vec3 v2		=	pAbsPose[HeadIdx  ].q.GetColumn1();
	Vec3 r0=(v1+v2).GetNormalized();
	Vec3 r1=r0;		r1.z=0; r1.Normalize(); 
	Vec3 PivotAxisX= rPhysLocationNext.q * (r0+4*r1).GetNormalized();


	Vec3 LocalLookAtTarget = m_LookIKTargetSmooth-rPhysLocationNext.t;
	Vec3 LeftEyePos		= (pAbsPose[lidx]*m_ql).t;
	Vec3 RightEyePos	= (pAbsPose[ridx]*m_qr).t;
	Vec3 MiddleEyePos	= (LeftEyePos+RightEyePos)*0.5f;
	
//	ColorF colorl( 1.0f, 0.0f, 0.0f, 1.0f );
//	g_pAuxGeom->DrawSphere(LeftEyePos, 0.003f, colorl);
//	ColorF colorr( 0.0f, 0.0f, 1.0f, 1.0f );
//	g_pAuxGeom->DrawSphere(RightEyePos, 0.003f, colorr);
//	ColorF colorg( 0.0f, 1.0f, 0.0f, 1.0f );
//	g_pAuxGeom->DrawSphere(MiddleEyePos, 0.003f, colorg);
//	return;

	Vec3 LookDir = (LocalLookAtTarget-MiddleEyePos).GetNormalized();

	f32 dot	=	PivotAxisX|LookDir;
	extern f32 g_YLine;
//	float fColor[4] = {1,1,0,1};
	//g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.3f, fColor, false,"dot: %12.8f", dot );	g_YLine+=16.0f;

	f32 FOR = cosf(m_LookIKFOR);
	if (dot<FOR || m_LookIKFadeout)
	{
		//avoid Linda-Blair effect
		m_LookIKFadeout=1;
		m_LookIKBlend-=BINOUT*fabsf(m_pInstance->m_fOriginalDeltaTime);
		if (m_LookIKBlend<0.0f)
		{
			m_LookIKFadeout=0;
			m_LookIKBlend=0.0f;
			return false;
		}
	}
	else
	{
		if (LIKFlag)
		{
			m_LookIKBlend+=BINOUT*fabsf(m_pInstance->m_fOriginalDeltaTime);
			if (m_LookIKBlend>1.0f)	m_LookIKBlend=1.0f;
			assert(m_LookIKFadeout==0);
		}
		else
		{
			m_LookIKBlend-=BINOUT*fabsf(m_pInstance->m_fOriginalDeltaTime);
			if (m_LookIKBlend<0.0f)	m_LookIKBlend=0.0f;
			assert(m_LookIKFadeout==0);
		}
	}

/*	
	extern f32 g_YLine;
	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.3f, fColor, false,"m_LookIKBlend: %12.8f", m_LookIKBlend );	g_YLine+=16.0f;
	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.3f, fColor, false,"Model: %s ",m_pInstance->GetModelFilePath());	g_YLine+=16.0f;
	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.3f, fColor, false,"Target:(%12.8f %12.8f %12.8f) ",m_LookIKTarget.x, m_LookIKTarget.y, m_LookIKTarget.z );	g_YLine+=16.0f;
	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.3f, fColor, false,"LookIK_FOR: %12.8f    SmoothTarget:(%12.8f %12.8f %12.8f) ",m_LookIKFOR, m_LookIKTargetSmooth.x, m_LookIKTargetSmooth.y, m_LookIKTargetSmooth.z );	g_YLine+=16.0f;
	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.3f, fColor, false,"m_fOriginalDeltaTime: %12.8f",m_pInstance->m_fOriginalDeltaTime );	g_YLine+=16.0f;
*/
	
	for (uint32 i=0; i<numJoints; i++)
		pRelativeQuatIK[i] = pRelPose[i];

//	Matrix34 rRenderMat34 = Matrix34(Matrix33(IDENTITY),rPhysLocationNext.t);
//	pSkeletonPose->DrawSkeleton( rRenderMat34, 0 );

	Vec3 CamPos	=  LocalLookAtTarget * rPhysLocationNext.q;
	Vec3 vLookAt	= (CamPos-MiddleEyePos).GetNormalized();


#if !defined(__SPU__)
	if (Console::GetInst().ca_DrawLookIK)
	{
		IRenderAuxGeom* pAux = gEnv->pRenderer->GetIRenderAuxGeom();
		if (pAux)
		{
			ColorF color( 1.0f, 0.0f, 0.0f, m_LookIKBlend );
			pAux->DrawLine(rPhysLocationNext*MiddleEyePos, color, m_LookIKTargetSmooth, color);
			pAux->DrawSphere(m_LookIKTargetSmooth, 0.3f, color);
		}
		
		gEnv->pRenderer->DrawLabel
			( 
			m_LookIKTargetSmooth + Vec3(0,0,0.4f), 1.0f, "t:(%.1f %.1f %.1f)\nm:(%.1f %.1f %.1f)\nl:(%.1f %.1f %.1f)",
			rPhysLocationNext.t.x,
			rPhysLocationNext.t.y,
			rPhysLocationNext.t.z,
			m_LookIKTargetSmooth.x,
			m_LookIKTargetSmooth.y,
			m_LookIKTargetSmooth.z,
			MiddleEyePos.x,
			MiddleEyePos.y,
			MiddleEyePos.z
			);
	}
#endif


	CFacialInstance *pFacialInstance= SPU_MAIN_PTR( (CFacialInstance *)(m_pInstance->CCharInstance::GetFacialInstance()) );
 	if (pFacialInstance)
	{
		// Moved to the CSkeletonAnimTask::Execute() function, so that blinking can happen without LookIK being active, too
		//pFacialInstance->CFacialInstance::ApplyProceduralFaceBehaviour(CamPos);

		// Instead only adjust the camera position here
		pFacialInstance->CFacialInstance::AdjustCamPosForProceduralFaceAnimation(CamPos);
	}

	//VDirIdentity is the matrix that is not changing the rotation of the head
	Quat defaultAbsHeadJoint = pSkeletonPose->GetDefaultAbsJointByID(HeadIdx).q;
	Vec3 headX=defaultAbsHeadJoint.GetColumn0();
	Vec3 headY=defaultAbsHeadJoint.GetColumn1();
	Vec3 headZ=defaultAbsHeadJoint.GetColumn2();
	Matrix33 VDirIdentity = Matrix33::CreateFromVectors( -headZ, headY, headX ); 

	Quat AlignmentQuat	 =	 !Quat(VDirIdentity) * defaultAbsHeadJoint;  

	Quat absHeadQuat = Quat::CreateRotationVDir(vLookAt) * AlignmentQuat;
	Quat relHeadQuat = !pAbsPose[HeadIdx].q * absHeadQuat ;
	relHeadQuat.NormalizeSafe();

//	float fColor[4] = {1,1,0,1};
//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.3f, fColor, false,"relHeadQuat: (%12.8f %12.8f %12.8f %12.8f) ",relHeadQuat.w,relHeadQuat.v.x, relHeadQuat.v.y, relHeadQuat.v.z );	g_YLine+=16.0f;
//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.3f, fColor, false,"pAbsPose[0]:  rot (%12.8f %12.8f %12.8f %12.8f) pos (%12.8f %12.8f %12.8f) ",pAbsPose[0].q.w,pAbsPose[0].q.v.x, pAbsPose[0].q.v.y, pAbsPose[0].q.v.z,    pAbsPose[0].t.x,pAbsPose[0].t.y,pAbsPose[0].t.z );	g_YLine+=16.0f;
//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.3f, fColor, false,"rPhysLocationNext:  rot (%12.8f %12.8f %12.8f %12.8f) ",rPhysLocationNext.q.w,rPhysLocationNext.q.v.x, rPhysLocationNext.q.v.y, rPhysLocationNext.q.v.z );	g_YLine+=16.0f;


	ComputeNewSpineAndHeadOrientation(params, pRelativeQuatIK, relHeadQuat);

	CalculateEyeLookDirection(params, CamPos, pRelativeQuatIK);

	//-------------------------------------------------------------------------------
	//---     blend with the FK-pose and calculate the new absolute pose         ---
	//-------------------------------------------------------------------------------
	m_IKBlend = clamp(m_LookIKBlend,0.0f,1.0f);
	m_IKBlend -= 0.5f;
	m_IKBlend = 0.5f + m_IKBlend/(0.5f+2.0f*m_IKBlend*m_IKBlend);

	for (uint32 i=1; i<numJoints; i++)
	{
		Quat quat0 = pRelPose[i].q;
		Quat quat1 = pRelativeQuatIK[i].q;
		pRelPose[i].q.SetNlerp(quat0,quat1,m_IKBlend);

		int16 p = pCharacterModelSkeleton->GetJointParentIDByID(i);
		pAbsPose[i]	= pAbsPose[p] * pRelPose[i];
		assert(pAbsPose[i].IsValid());
	}

	if (m_LookIKFadeout==0)
	{
		assert((uint32)Spine1Idx < numJoints && (uint32)Spine2Idx < numJoints && (uint32)Spine3Idx < numJoints && (uint32)NeckIdx < numJoints && (uint32)HeadIdx < numJoints);
		if (Spine1Idx>0) m_oldSpine1 = pRelativeQuatIK[Spine1Idx].q;
		if (Spine2Idx>0) m_oldSpine2 = pRelativeQuatIK[Spine2Idx].q;
		if (Spine3Idx>0) m_oldSpine3 = pRelativeQuatIK[Spine3Idx].q;
		if (NeckIdx>0) m_oldNeck = pRelativeQuatIK[NeckIdx].q;;
		if (HeadIdx>0) m_oldHead = pRelativeQuatIK[HeadIdx].q;
	}

	for (uint32 j=0; j<params.jointCount; j++)
	{
		assert( params.pPoseRelative[j].q.IsUnit() );
		assert( params.pPoseAbsolute[j].q.IsUnit() );
		assert( params.pPoseRelative[j].IsValid() );
		assert( params.pPoseAbsolute[j].IsValid() );
	}

	return true;
}
#endif

//

namespace Util {

	SPU_NO_INLINE void GetJointID( EAnimationJointType type, int32 &rIndex, int32 &rParentIndex, ICharacterModelSkeleton* pCharacterModelSkeleton )
	{
		rIndex = pCharacterModelSkeleton->GetJointIDByType(type);		
		if (rIndex>0) 
			rParentIndex=pCharacterModelSkeleton->GetJointParentIDByID(rIndex);
	}

	SPU_NO_INLINE void Update( int32 nIndex, int32 nParentIndex, Quat &relHeadQuat, f32 fBlendSmooth, QuatT *pRelPose, QuatT *pAbsPose )
	{
		if( nIndex < 0 )
			return;

		Quat lerp	= Quat::CreateNlerp( Quat(IDENTITY), relHeadQuat, fBlendSmooth );
		pRelPose[nIndex].q *= lerp;
		pAbsPose[nIndex] = pAbsPose[nParentIndex] * pRelPose[nIndex];			
	}

	SPU_NO_INLINE void FadeOut( int32 nIndex, int32 nParentIndex, Quat &rOldRotation, QuatT *pRelPose, QuatT *pAbsPose)
	{
		if( nIndex < 0 )
			return;
		
		pRelPose[nIndex].q	=	rOldRotation;
		pAbsPose[nIndex] = pAbsPose[nParentIndex] * pRelPose[nIndex];			
	}
}

SPU_NO_INLINE void CLookAt::ComputeNewSpineAndHeadOrientation(const SAnimationPoseModiferParams& params, QuatT* pRelativeQuatIK, Quat &relHeadQuat)
{
	ICharacterModelSkeleton* pCharacterModelSkeleton = params.GetICharacterModelSkeleton();

	// use local pointer so spare dereferencing
	//	int16* 	const __restrict pIdxArray 			= m_pModelSkeleton->m_IdxArray;

	QuatT* 	const	__restrict pRelPose = SPU_LOCAL_PTR( params.pPoseRelative );
	QuatT* 	const	__restrict pAbsPose = SPU_LOCAL_PTR( params.pPoseAbsolute );

	//	int32 Spine0Idx	=	pIdxArray[eIM_Spine0Idx];
	//	int32 pSpine0Idx=-1;	
	//	if (Spine0Idx>0) pSpine0Idx=m_parrModelJoints[Spine0Idx].m_idxParent;

	int32 Spine1Idx, pSpine1Idx=-1;	
	int32 Spine2Idx, pSpine2Idx=-1;
	int32 Spine3Idx, pSpine3Idx=-1;
	int32 NeckIdx, pNeckIdx=-1;
	int32 HeadIdx, pHeadIdx=-1;

	Util::GetJointID( eIM_Spine1Idx, Spine1Idx, pSpine1Idx, pCharacterModelSkeleton );
	Util::GetJointID( eIM_Spine2Idx, Spine2Idx, pSpine2Idx, pCharacterModelSkeleton );
	Util::GetJointID( eIM_Spine3Idx, Spine3Idx, pSpine3Idx, pCharacterModelSkeleton );
	Util::GetJointID( eIM_NeckIdx, NeckIdx, pNeckIdx, pCharacterModelSkeleton );
	Util::GetJointID( eIM_HeadIdx, HeadIdx, pHeadIdx, pCharacterModelSkeleton );								

	//-----------------------------------------------------------------------
	//---           new Spline1 orientation      -------------------------------
	//-----------------------------------------------------------------------
	Util::Update( Spine1Idx, pSpine1Idx, relHeadQuat, m_lookIKBlendsSmooth[0], pRelativeQuatIK, pAbsPose );	

	//-----------------------------------------------------------------------
	//---           new Spline2 orientation      -------------------------------
	//-----------------------------------------------------------------------
	Util::Update( Spine2Idx, pSpine2Idx, relHeadQuat, m_lookIKBlendsSmooth[1], pRelativeQuatIK, pAbsPose );	

	//-----------------------------------------------------------------------
	//---           new Spline3 orientation      ----------------------------
	//-----------------------------------------------------------------------
	Util::Update( Spine3Idx, pSpine3Idx, relHeadQuat, m_lookIKBlendsSmooth[2], pRelativeQuatIK, pAbsPose );	
	
	//-----------------------------------------------------------------------
	//---           new Neck orientation      -------------------------------
	//-----------------------------------------------------------------------
	Util::Update( NeckIdx, pNeckIdx, relHeadQuat, m_lookIKBlendsSmooth[3], pRelativeQuatIK, pAbsPose );	


	//apply additional relative head orientation (this is comming from the facial-anim system)
	pRelPose[HeadIdx].q		= pRelPose[HeadIdx].q *m_addHead;
	pRelativeQuatIK[HeadIdx].q	= pRelativeQuatIK[HeadIdx].q*m_addHead;

	//-----------------------------------------------------------------------
	//---           new Head orientation      -------------------------------
	//-----------------------------------------------------------------------
	Quat lerpHead		= Quat::CreateNlerp( Quat(IDENTITY), relHeadQuat, m_lookIKBlendsSmooth[4]/1.8f);
	pRelativeQuatIK[HeadIdx].q *= lerpHead;
	pAbsPose[HeadIdx] = pAbsPose[pHeadIdx] * pRelativeQuatIK[HeadIdx];
	pAbsPose[HeadIdx].q.NormalizeSafe();

	if (m_LookIKFadeout)
	{
		//we are in FadeOut-Mode. Ignore all Look-IK caculation we did for this frame and just take the last valid values
		Util::FadeOut( Spine1Idx, pSpine1Idx, m_oldSpine1, pRelativeQuatIK, pAbsPose );
		Util::FadeOut( Spine2Idx, pSpine2Idx, m_oldSpine2, pRelativeQuatIK, pAbsPose );
		Util::FadeOut( Spine3Idx, pSpine3Idx, m_oldSpine3, pRelativeQuatIK, pAbsPose );
		Util::FadeOut( NeckIdx, pNeckIdx, m_oldNeck, pRelativeQuatIK, pAbsPose );
		Util::FadeOut( HeadIdx, pHeadIdx, m_oldHead, pRelativeQuatIK, pAbsPose );
	}

}

SPU_NO_INLINE void CLookAt::CalculateEyeLookDirection(const SAnimationPoseModiferParams& params, Vec3 &CamPos, QuatT* pRelativeQuatIK)
{
	ICharacterModelSkeleton* pCharacterModelSkeleton =	params.GetICharacterModelSkeleton();

	QuatT* const __restrict pRelPose = SPU_PTR_SELECT(params.pPoseRelative, gSkeletonRelativePose);
	QuatT* const __restrict pAbsPose = SPU_PTR_SELECT(params.pPoseAbsolute, gSkeletonAbsolutePose);

	int32 HeadIdx = pCharacterModelSkeleton->GetJointIDByType(eIM_HeadIdx);
	int32 ridx = pCharacterModelSkeleton->GetJointIDByType(eIM_RightEyeIdx);
	int32 lidx = pCharacterModelSkeleton->GetJointIDByType(eIM_LeftEyeIdx);

	// NOTE: Quat::CreateRotationVDir() will assume an implicit up vector along
	// the Z axis. This will cause the eyes to rotate along their forward Y
	// axis whenever the PoseModifier is enabled.
	// In order to avoid this we rotate the Y axis back minus 45 degrees.
	// TODO: Better solution would be to provide a way to create a rotation out
	// of a direction and an arbitrary up axis.
	Quat rotYMinus45 = Quat::CreateRotationY(-gf_PI*0.5f);

	QuatT LAbsolutePose = (pAbsPose[HeadIdx] * pRelPose[lidx]) * m_ql;
	m_oldEyeLeft = m_addLEye;
	Vec3 CamPosL = CamPos-LAbsolutePose.t;
	Quat absLEye = Quat::CreateRotationVDir(CamPosL.GetNormalized()) * rotYMinus45 * m_addLEye;
	m_additionalRotationLeft = !pAbsPose[HeadIdx].q * absLEye;

	QuatT RAbsolutePose = (pAbsPose[HeadIdx] * pRelPose[ridx]) * m_qr;
	m_oldEyeRight = m_addREye;
	Vec3 CamPosR = CamPos-RAbsolutePose.t;
	Quat absREye = Quat::CreateRotationVDir(CamPosR.GetNormalized()) * rotYMinus45 * m_addREye;
	m_additionalRotationRight = !pAbsPose[HeadIdx].q * absREye;
}


